/* VxWorks API includes */
#include "vxWorks.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "ioLib.h"
#include "semLib.h"
#include <taskLib.h>
#include <logLib.h>
#include "intLib.h"
#include "vmLib.h"   /* added to support */
#include "iv.h"

/* pcPentium BSP includes */
#include <sysLib.h>
#include "btvid.h"

/* stereo vision */
#define ST_FOCAL_LENGTH 545   /*13.1cm*/
#define ST_B_DIST       11   /*10.9cm*/
#define ST_COUNT_RATE   15/3
UINT8  st_count;
int       run_stereo;

struct STEREOcent{
   int centx;
   int centy;
} STCent;


#define ICDFRAME1 1
#define ICDFRAME2 2

#define ICD_N 320
#define ICD_M 240

#define ICD_UNIT_TEST    /* ICD UNIT TESTING ONLY!!!!!! COMMENT OUT WHEN USING WITH BT*&* driver */

/*#ifndef ICD_UNIT_TEST */
extern UINT8 savedFrameBuffer[320 * 240];
extern UINT8 savedFrameBuffer2[320 * 240];
extern SEM_ID semFrame1;
extern SEM_ID semFrame2;
/* 
 UINT8 savedFrameBuffer[320*240];
 UINT8 savedFrameBuffer2[320*240];
 SEM_ID semFrame1;
 SEM_ID semFrame2;	
 #endif*/

UINT8 ICDframe[ICD_N * ICD_M];
UINT8 convframe1[ICD_N * ICD_M];
UINT8 convframe2[ICD_N * ICD_M];

int count = 0;

extern SEM_ID frameRdy;
SEM_ID cent1Rdy;
SEM_ID cent2Rdy;

struct ICDcent {
	int centx1;
	int centy1;
	int centx2;
	int centy2;
} ICDSharedCent;

int run_ICD = 1;

#define ICD_WINDOW_SIZE 3

/* fill up rows then columns */

const int g_coeff[ICD_WINDOW_SIZE][ICD_WINDOW_SIZE] = { { 2, 2, 2 },
		{ 0, 0, 0 }, { 2, 2, 2 } };
const int gscale = 32;

int getCenterX1 ( void ) {
	return ICDSharedCent.centx1;
}
int getCenterX2 ( void ) {
	return ICDSharedCent.centx2;
}
int getCenterY1 ( void ) {
	return ICDSharedCent.centy1;
}
int getCenterY2 ( void ) {
	return ICDSharedCent.centy2;
}


/* void ICD2Dconv(UINT8* pframe)
 * 
 * Convolution on the pointer frame
 *  Copies the frame from frame buffer and process
 *   output of convolution is stored in convframe
 */
void ICD2Dconv(UINT8* pframe, UINT8* poutframe) {
	int i, j, ii, jj;
	float data, coeff, sum;

	/*Take frame from BT878 driver buffer to ICDframe for process 
	 memcpy(pframe, &ICDframe, ICD_N*ICD_M);*/

	/* in, out pointers to m x n images (UINT frame)
	 g_coeffs[K][K] is a 2D array of integer coefficients
	 scale is a scaling factor to normalise the filter gain
	 */
	for (i = ICD_WINDOW_SIZE / 2; i < ICD_M - ICD_WINDOW_SIZE / 2; ++i) /* iterate through image */
	{
		for (j = ICD_WINDOW_SIZE / 2; j < ICD_N - ICD_WINDOW_SIZE / 2; ++j) {
			sum = 0; /* sum will be the sum of input data * coeff terms */

			for (ii = -ICD_WINDOW_SIZE / 2; ii <= ICD_WINDOW_SIZE / 2; ++ii) /* iterate over kernel*/
			{
				for (jj = -ICD_WINDOW_SIZE / 2; jj <= ICD_WINDOW_SIZE / 2; ++jj) {
					data = pframe[(i + ii) * ICD_N + j + jj];
					coeff = g_coeff[ii + ICD_WINDOW_SIZE / 2][jj
							+ ICD_WINDOW_SIZE / 2];

					sum += (data * coeff) / gscale;

				}
			}
			poutframe[i * ICD_N + j] = sum; /* scale sum of convolution products and store in output */
		}
	}

}

/* void ICDenergyDetect(int* x, int* y);
 * 
 * Finds the average location of maximum energy
 *  in x,y coordinates
 */
void ICDenergyDetect(int* x, int* y, UINT8* pframe) {
	int i, max_ind, x_cor, y_cor;
	int x_max, x_min, y_max, y_min;
	UINT8 max;

	/* determine the max */
	max = pframe[0];
	x_max = 0;
	x_min = ICD_N;
	y_max = 0;
	y_min = ICD_M;

	for (i = 0; i < (ICD_N * ICD_M); i++) {
		if (pframe[i] > max) {
			max = pframe[i];
			max_ind = i;
		}
	}

	/* average routine of max/min indicies */
	for (i = 0; i < (ICD_N * ICD_M); i++) {
		if(pframe[i] == max){
			x_cor = (i % ICD_N);
			y_cor = (i /ICD_N);

			if(x_cor < x_min)
				x_min = x_cor;
			if(x_cor > x_max)
				x_max = x_cor;
			if(y_cor < y_min)
				y_min = y_cor;
			if(y_cor > y_max)
				y_max = y_cor;
		}
	}

	/*logMsg("Energy Detection estimate [max %i] [indici %i]\n", max, max_ind, 0,
			0, 0, 0);*/

/*	corner detection
	*x = (int) (max_ind % ICD_N);
	*y = (int) (max_ind / ICD_N);
*/
	/* center rotuine */
	 *x = (int)((x_min+x_max)/2);
	 *y = (int)((y_min+y_max)/2); 

	/*logMsg("X [max %i] [min %i] Y [max %i] [min %i] \n",x_max,x_min,y_max,y_min,0,0);*/
	 
}

/* ICDframe1_task
 * Use the Y comoponent of NTSC image as grayscale image
 * 2D convolution with g coefficent matrix to do edge detection/filtering
 */
void ICDframe1_task(void) {
	while (run_ICD) {
		while (count < 10) {
			semTake(semFrame1, WAIT_FOREVER);
			count++;

			logMsg("Convolute Frame 1 \n", 0, 0, 0, 0, 0, 0);
			ICD2Dconv(savedFrameBuffer, convframe1);

			logMsg("Detect Energy of Original Frame 1\n", 0, 0, 0, 0, 0, 0);
			ICDenergyDetect(&ICDSharedCent.centx1, &ICDSharedCent.centy1,
					savedFrameBuffer);
			logMsg("Centroid of frame 1: x:%i y:%i \n", ICDSharedCent.centx1,
					ICDSharedCent.centy1, 0, 0, 0, 0);

			logMsg("Detect Energy of Convoluted Frame 1 \n", 0, 0, 0, 0, 0, 0);
			ICDenergyDetect(&ICDSharedCent.centx1, &ICDSharedCent.centy1,
					convframe1);

			logMsg("Centroid of frame 1: x:%i y:%i \n", ICDSharedCent.centx1,
					ICDSharedCent.centy1, 0, 0, 0, 0);
			semGive(cent1Rdy);
		}
	}
}

void ICDProcessFrame1(void) {
	wvEvent(2, 0, 0);
	count++;
	ICD2Dconv(savedFrameBuffer, convframe1);
	wvEvent(88,0,0);
	ICDenergyDetect(&ICDSharedCent.centx1, &ICDSharedCent.centy1, convframe1);
	semGive(cent1Rdy);
	wvEvent(3, 0, 0);
}

void ICDProcessFrame2(void) {
	wvEvent(2, 0, 0);
	ICD2Dconv(savedFrameBuffer2, convframe2);
	wvEvent(89,0,0);
	ICDenergyDetect(&ICDSharedCent.centx2, &ICDSharedCent.centy2, convframe2);
	semGive(cent2Rdy);
	wvEvent(3, 0, 0);
}

/* ICDframe2_task
 * Use the Y comoponent of NTSC image as grayscale image
 * 2D convolution with g coefficent matrix to do edge detection/filtering
 */
void ICDframe2_task(void) {
	while (run_ICD) {
		semTake(semFrame2, WAIT_FOREVER);

		ICD2Dconv(savedFrameBuffer2, convframe2);

		ICDenergyDetect(&ICDSharedCent.centx2, &ICDSharedCent.centy2,
				convframe2);
		logMsg("centroid of frame 2: x:%i y:%i ", ICDSharedCent.centx2,
				ICDSharedCent.centy2, 0, 0, 0, 0);
		semGive(cent2Rdy);
	}
}

/* initICD(void)
 * 
 * Initialize intermediate convolution buffers, 
 * semaphore for centroid
 * 
 */
void initICD(void) {
	run_ICD = 1;
	cent1Rdy = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
	cent2Rdy = semBCreate(SEM_Q_FIFO, SEM_EMPTY);

	memset(convframe1, 0, sizeof(convframe1));
	memset(convframe1, 0, sizeof(convframe1));
	
	/*if (taskSpawn("ICDframe1_task", 90, 0, (4 * 1024),
			(FUNCPTR) ICDframe1_task, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR) {
		printf("Release ICDframe1_task spawn failed\n");
	} else
		printf("Release ICDframe1_task spawned\n");
	*/
	/*if(taskSpawn("ICDframe2_task", 50, 0, (4*1024), (FUNCPTR)ICDframe2_task, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
	 == ERROR) {
	 printf("Release ICDframe2_task spawn failed\n");
	 }
	 else
	 printf("Release ICDframe2_task task spawned\n"); 
	 */
	return;
}

void stopICD(void) {
	run_ICD = 0;
	semDelete(cent1Rdy);
	semDelete(cent2Rdy);
}

void testICDCorner(void) {
	initICD();
	semFrame1 = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
	semFrame2 = semBCreate(SEM_Q_FIFO, SEM_EMPTY);

	memset(savedFrameBuffer, 0, sizeof(savedFrameBuffer));

	/*Set a single pixel in image to 255*/

	savedFrameBuffer[0] = 128;
	savedFrameBuffer[(161 * 320) + 121] = 255;

	semGive(semFrame1);

}

void testICDCenter(void)
{
	initICD();
	semFrame1 = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
	semFrame2 = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
	
	memset(	savedFrameBuffer,0,sizeof(savedFrameBuffer));
	
	/*Set a single pixel in image to 255*/
	
	savedFrameBuffer[0] = 128;
	
	/* 4 corners */
	savedFrameBuffer[(160*320)+120] = 255;
	savedFrameBuffer[(163*320)+120] = 255;
	savedFrameBuffer[(160*320)+123] = 255;
	savedFrameBuffer[(163*320)+123] = 255;
	semGive(semFrame1);
	
}
/*
int tempd1;
int tempd2;*/

float st_cent_delta_y;
float st_cent_delta_x;
float st_d_y;
float st_d_x;
float st_d ;
float st_depth;

float delta1;
float delta2;

void stereoTask(void)
{

   /* synchronize to both semaphore */

 /*  tempd1 = (ICDSharedCent.centy1 - ICD_N/2);
   tempd1 = (ICDSharedCent.centy2 - ICD_N/2);

   logMsg("deltay1 %i, deltay2 %i \n",tempd1, tempd2,0,0,0,0);
 */		     
   delta1 = abs((float)(ICDSharedCent.centy1 - ICD_N/2));
   delta2 = abs((float)(ICDSharedCent.centy2 - ICD_N/2));
   
   st_cent_delta_y = delta1 + delta2;
   
   delta1 = abs((float)(ICDSharedCent.centx1 - ICD_M/2));
   delta2 = abs((float)(ICD_M/2 - ICDSharedCent.centx2));
		   
   st_cent_delta_x = delta1 + delta2;
   
   /* Apply stereo range equation (cent_delta = b*f/d) */
   if(st_cent_delta_y != 0)
	   st_d_y = ST_B_DIST * ST_FOCAL_LENGTH / st_cent_delta_y;
   else
	   st_d_y = 10000;
	  
   if(st_cent_delta_x != 0)
	   st_d_x = ST_B_DIST * ST_FOCAL_LENGTH / st_cent_delta_x;
   else
	   st_d_x = 10000;
   
   /* depth using two dimension */
   st_d = st_d_y * st_d_y + st_d_x * st_d_x;  
   st_depth = sqrt(st_d);
   
   /*STCent.centx = (ICDSharedCent.centx1 + ICDSharedCent.centx2) / 2;*/
   /*STCent.centy = (ICDSharedCent.centy1 + ICDSharedCent.centy2) / 2;*/
   
   /*st_cent_delta_y = (float)9.5;
   st_cent_delta_x = 9.5;
   st_depth = 100;*/
   
 /*  logMsg("[Depth %f] [x delta %f] [y delta %f] [depth x %f] [depth y %f] \n",st_depth,
		   st_cent_delta_x,st_cent_delta_y,st_d_x,st_d_y,0);
   */
  /* printf("[Depth %f] [x delta %f] [y delta %f] [dx %f] [dy %f] \n",st_depth,
   		   st_cent_delta_x,st_cent_delta_y,st_d_x,st_d_y);*/
  printf("[Depth %f] \n",st_d_x);
}
